package Code;
import java.util.List;
import java.util.Random;


public enum CalculateTraversalPotentials 
{;
	private static double minDistance = 2;
	private static double obstacleRadiusOfInfluence = 40;
	private static double goalRadiusOfInfluence = 50;
	
	/************************************************************
	 * Calculate the potential field for a given pt and goal
	 * 		Returns the new point to move toward
	 * @param currPt the current pt
	 * @param goal the goal
	 * @param obstacles a list of obstacles on the field
	 * @return the potential (x & y) as a point
	 **********************************************************/
	public static Point calcPotentialPt(Point currPt, Point goal, List<Point> obstacles, int worldDimensions) 
	{
		//calc the attraction
		Point potential = calcAttraction(currPt, goal);
		System.out.println("pot " + potential.toString());
		double x_attraction = potential.getX();
		double y_attraction = potential.getY();
		Point resistance = calcResistance(currPt, obstacles);
		System.out.println("res " + resistance.toString());
		double x_resistance = resistance.getX();
		double y_resistance = resistance.getY();
		Point tangential = calcTangential(currPt, obstacles, goal);
		System.out.println("tan " + tangential.toString());
		double x_tangential = tangential.getX();
		double y_tangential = tangential.getY();
		
		double x_random = 0;
		double y_random = 0;
		if (Math.abs(x_resistance) > 0.001 || Math.abs(y_resistance) > 0.001)
		{
			Point random = calcRandom(currPt, worldDimensions, obstacles);
			x_random = random.getX();
			y_random = random.getY();
		}
		
		double xPotential = x_attraction + x_resistance + x_tangential + x_random;
		double yPotential = y_attraction + y_resistance + y_tangential + y_random;
		
//		System.out.println("Point: (" + currPt.getX() + ", " + currPt.getY() + ")\tresistance (" +
//				x_resistance + ", " + y_resistance + ")\ttangential ( " + x_tangential + ", " + 
//				y_tangential + ")\ttotal (" + xPotential + ", " + yPotential + ")");
		return new Point(xPotential + currPt.getX(), yPotential + currPt.getY());
	}
	
	/***************************************************************
	 * Calculate the resistance field for a location
	 * @param currPt	the current location
	 * @param obstacles	the obstacles on the field
	 * @return	the resistance in the x direction
	 *****************************************************************/
	public static Point calcResistance(Point currPt, List<Point> obstacles)
	{
		double thetaObstacle;
		double distToObstacle;
		double x_resistance = 0;
		double y_resistance = 0;
		int count = 0;
		double minX_resistance = 0;
		double minY_resistance = 0;
		double tempX_resistance = 0;
		double tempY_resistance = 0;
		double minDist = Double.MAX_VALUE;

		for (Point obst : obstacles)
		{
			thetaObstacle = calcAngle(currPt, obst);		//theta for obstacleClosestPt
			distToObstacle = Math.max(obst.calcDistance(currPt), minDistance);

			if (Double.compare(distToObstacle, obstacleRadiusOfInfluence) < 0)
			{
//				System.out.println("res loop");
				tempX_resistance = (distToObstacle - obstacleRadiusOfInfluence)*Math.cos(thetaObstacle);
				tempY_resistance = (distToObstacle - obstacleRadiusOfInfluence)*Math.sin(thetaObstacle);
				x_resistance += tempX_resistance;
				y_resistance += tempY_resistance;
				count ++;
				if (Double.compare(distToObstacle,  minDist) < 0)
				{
					minX_resistance = tempX_resistance;
					minY_resistance = tempY_resistance;
					minDist = distToObstacle;
				}
			}
		}
		if (count != 0)
		{
			//x_resistance /=(double)count/2;
			//y_resistance /= (double)count/2;
		}
//		double multiplier = 2;
//		minX_resistance *= multiplier;
//		minY_resistance *= multiplier;
//		x_resistance = (x_resistance + minX_resistance)/(multiplier + 1);
//		y_resistance = (y_resistance + minY_resistance)/(multiplier + 1);
		x_resistance += minX_resistance;
		y_resistance += minY_resistance;
		return new Point(x_resistance/2, y_resistance/2);
	}
	
	/****************************************************************
	 * Calculate the attraction field for a given point and goal
	 * @param currPt	current location
	 * @param goal		the goal
	 * @return			the attraction at a given pt
	 **************************************************************/
	public static Point calcAttraction(Point currPt, Point goal)
	{
		double thetaGoal = calcAngle(currPt, goal);
		double distToGoal = Math.max(currPt.calcDistance(goal), minDistance);
		if (Double.compare(distToGoal,goalRadiusOfInfluence) <= 0)
		{
			return new Point(distToGoal*Math.cos(thetaGoal), distToGoal*Math.sin(thetaGoal));
		}
		else
			return new Point (goalRadiusOfInfluence*Math.cos(thetaGoal), goalRadiusOfInfluence*Math.sin(thetaGoal));
	}
	
	/******************************************************************
	 * Calculate the tangential field for a given point
	 * @param currPt	the current location
	 * @param obstacles	the obstacles on the field
	 * @return		the attraction at the given pt
	 ******************************************************************/
	public static Point calcTangential(Point currPt, List<Point> obstacles, Point goal)
	{
		double thetaObstacle;
		double distToObstacle;
		double x_tangential = 0;
		double y_tangential = 0;
		int count = 0;
		double minX_tangential = 0;
		double minY_tangential = 0;
		double tempX_tangential = 0;
		double tempY_tangential = 0;
		double minDist = Double.MAX_VALUE;
		
		for (Point obst : obstacles)
		{
			thetaObstacle = calcAngle(currPt, obst);		//theta for obstacleClosestPt
			thetaObstacle = adjustTheta(thetaObstacle);
			distToObstacle = Math.max(obst.calcDistance(currPt), minDistance);

			if (Double.compare(distToObstacle, obstacleRadiusOfInfluence) < 0)
			{
//				System.out.println("tan loop");
				tempX_tangential = (distToObstacle - obstacleRadiusOfInfluence)*Math.cos(thetaObstacle);
				tempY_tangential = (distToObstacle - obstacleRadiusOfInfluence)*Math.sin(thetaObstacle);
//				System.out.println(x_tangential + " " + y_tangential);
				x_tangential += tempX_tangential;
				y_tangential += tempY_tangential;
				count ++;
//				System.out.println(x_tangential + " " + y_tangential);

				if (Double.compare(distToObstacle,  minDist) < 0)
				{
					minX_tangential = tempX_tangential;
					minY_tangential = tempY_tangential;
					minDist = distToObstacle;
				}
			}
		}
		if (count != 0)
		{
			//x_tangential /=(double)count/2;
			//y_tangential /= (double)count/2;
		}
//		double multiplier = 2d;
//		minX_tangential *= multiplier;
//		minY_tangential *= multiplier;
//		x_tangential = (x_tangential + minX_tangential)/(multiplier + 1);
//		y_tangential = (y_tangential + minY_tangential)/(multiplier + 1);
		x_tangential += minX_tangential;
		y_tangential += minY_tangential;
		return new Point(x_tangential/2, y_tangential/2);
	}
	
	private static double adjustTheta(double thetaObstacle) 
	{
		return thetaObstacle + Math.PI/2;
	}


	/*******************************************************************
	 * Calculate the angle between 2 points
	 * @param currPt  the first point
	 * @param goal    the second point
	 * @return		  the angle between 2 pts
	 ******************************************************************/
	public static double calcAngle(Point currPt, Point goal) 
	{
		return Math.atan2(goal.getY() - currPt.getY(), goal.getX()- currPt.getX());
	}
	
	public static Point calcRandom(Point currPt, int worldDimension, List<Point> obstacles)
	{
		Random rand = new Random();
		Point goal = new Point (0, 0);
		int x;
		int y;
		boolean stop = false;
		while(!stop)
		{
			x = rand.nextInt(worldDimension) - worldDimension/3;
			y = rand.nextInt(worldDimension) - worldDimension/3;
			goal = new Point (x, y);
			for(Point obst: obstacles)
			{
				if (!goal.equals(obst))
					stop = true;
			}
		}
		x = 0;
		y = 0;
		double theta = calcAngle(currPt, goal);
		double distToGoal = Math.max(currPt.calcDistance(goal), minDistance);
		if (Double.compare(distToGoal,  obstacleRadiusOfInfluence) <= 0)
		{
			return new Point(distToGoal*Math.cos(theta), distToGoal*Math.sin(theta));
		}
		else
			return new Point (0, 0);
	}
}
